Multibody dynamics modeling and numerical methods
%matplotlib inline
import time, os
import numpy as np
import sympy as sp
from scipy.integrate import solve_ivp
from IPython.display import display, Markdown
import matplotlib as mpl
from Models.models import *
from utils.SolutionDemo import *
sp.init_printing()
mpl.rcParams['figure.dpi'] = 50
repo_dir = os.path.dirname(os.getcwd())
m=[1, 1]
l=[1, 1]
Twobar_Model = ExplictModel(m=m, l=l,close_chain=False)
# y = np.append([0.5, 0, 0, 1.5, 0, 0], np.zeros(3*len(m)))
y0 = [None, None, np.pi/6, None, None, -np.pi/6]
y_dot = [0, None, None, 0.5, None, None]
y = np.concatenate(Twobar_Model.initial_condition(y0, y_dot))
g = np.tile([0, -9.81, 0], len(m))
f = [0, 0, 0, 0, 0, 0]
k = np.tile([1e6], 4)
Twobar_PModel = ApproximateModel(m=m, l=l, k=k, close_chain=False)
print_constrains(Twobar_Model)
print_govs(Twobar_Model, f)
ode = 'DOP853'
sol = solve_ivp(Twobar_Model.sim, [0, 10], y, method=ode, args=(f, g), t_eval=np.linspace(0, 10, 200))
Twobar = SolutionDemo(sol, m, l, rot=None)
multipliers = get_multipliers(Twobar_Model, f, g, sol, show=False)
print(Twobar.links.shape)
ode = 'DOP853'
solp = solve_ivp(Twobar_PModel.sim, [0, 10], y, method=ode, args=(f, g), t_eval=np.linspace(0, 10, 200))
Twobarp = SolutionDemo(solp, m, l, rot=None)
print(Twobarp.links.shape)
# Twobar.animate(title=ode, interval=50, save_as=repo_dir+'/imgs/DoublePendulum.gif')
Twobar.more_anim(multipliers, title='Double Pendulum', interval=50, save_as=repo_dir+'/imgs/DoublePendulum.mp4')